// import { Mat4 } from "./Mat4";
// import { Vec3 } from "./Vec3";
// import { Mat3 } from "./Mat3";

import { Transform } from "./Transform";
import { Vec3 } from "./Vec3";
import { Mat3 } from "./Mat3";
import { Quat } from "./Quat";
import { MathUtil } from "./MathUtil";

export class M {

    public static quat_getReal(q: Quat): number {
        // TODO: 不知道是干啥的
        return q.x + q.y + q.z + q.w;
    }

    public static vec3_abs(dst: Vec3, src: Vec3) {
        src.absTo(dst);
    }

    public static vec3_fromQuat(dst: Vec3, src: Quat) {
        dst.init(src.x, src.y, src.z);
    }

    public static mat3_mulRhsTransposed(dst: Mat3, src1: Mat3, src2: Mat3) {
        dst.init(

            src1.e00 * src2.e00 + src1.e01 * src2.e01 + src1.e02 * src2.e02,
            src1.e00 * src2.e10 + src1.e01 * src2.e11 + src1.e02 * src2.e12,
            src1.e00 * src2.e20 + src1.e01 * src2.e21 + src1.e02 * src2.e22,
            src1.e10 * src2.e00 + src1.e11 * src2.e01 + src1.e12 * src2.e02,
            src1.e10 * src2.e10 + src1.e11 * src2.e11 + src1.e12 * src2.e12,
            src1.e10 * src2.e20 + src1.e11 * src2.e21 + src1.e12 * src2.e22,
            src1.e20 * src2.e00 + src1.e21 * src2.e01 + src1.e22 * src2.e02,
            src1.e20 * src2.e10 + src1.e21 * src2.e11 + src1.e22 * src2.e12,
            src1.e20 * src2.e20 + src1.e21 * src2.e21 + src1.e22 * src2.e22

        );
    }

    public static quat_scale(dst: Quat, src: Quat, s: number) {
        src.scaleTo(s, dst);
    }

    public static quat_normalize(dst: Quat, src: Quat) {
        src.normalizeTo(dst);
    }

    public static quat_addRhsScaled(dst: Quat, src1: Quat, src2: Quat, src3: number) {
        src1.addScaleTo(src2, src3, dst);
    }

    public static quat_sub(dst: Quat, src1: Quat, src2: Quat) {
        src1.subTo(src2, dst);
    }

    public static quat_negate(dst: Quat, src: Quat) {
        src.negateTo(dst);
    }

    public static quat_dot(src1: Quat, src2: Quat): number {
        return src1.dot(src2);
    }

    public static quat_slerp(dst: Quat, src1: Quat, src2: Quat, t: number) {
        var qx: number;
        var qy: number;
        var qz: number;
        var qw: number;
        let q1 = src1.clone();
        let q2 = src2.clone();
        var d: number = M.quat_dot(q1, q2);
        if (d < 0) {
            d = -d;
            M.quat_negate(q2, q2);
        }
        if (d > 1 - 1e-6) {
            // two quaternions are too close, returns lerp instead
            var dq = new Quat();
            M.quat_sub(dq, q2, q1);
            M.quat_addRhsScaled(q2, q1, dq, t);
            M.quat_normalize(dst, q2);
        } else {
            // target angle
            var theta: number = t * MathUtil.acos(d);

            // make q2 orthogonal to this
            M.quat_addRhsScaled(q2, q2, q1, -d);
            M.quat_normalize(q2, q2);

            // mix them
            var sin: number = MathUtil.sin(theta);
            var cos: number = MathUtil.cos(theta);
            M.quat_scale(q1, q1, cos);
            M.quat_addRhsScaled(dst, q1, q2, sin);
        }
    }

    public static quat_id(q: Quat) {
        q.identity();
    }

    public static mat3_toEulerXyz(dst: Vec3, src: Mat3) {
        src.toEulerXyzTo(dst);
    }



    public static mat3_negate(dst: Mat3, src: Mat3) {
        src.negateTo(dst);
    }

    public static vec3_toCrossMatrix(dst: Mat3, src: Vec3) {
        dst.init(
            0, -src.z, src.y,
            src.z, 0, -src.x,
            -src.y, src.x, 0
        );
    }

    public static vec3_negate(dst: Vec3, src: Vec3) {
        src.negateTo(dst);
    }

    public static aabb_contains(min1: Vec3, max1: Vec3, min2: Vec3, max2: Vec3): boolean {
        return min1.x <= min2.x && max1.x >= max2.x &&
            min1.y <= min2.y && max1.y >= max2.y &&
            min1.z <= min2.z && max1.z >= max2.z;
    }

    public static vec3_normalize(dst: Vec3, src: Vec3) {
        src.normalzeTo(dst);
    }

    public static vec3_perp(dst: Vec3, src: Vec3) {
        var x1: number = src.x;
        var y1: number = src.y;
        var z1: number = src.z;
        var x2: number = x1 * x1;
        var y2: number = y1 * y1;
        var z2: number = z1 * z1;
        var d: number;
        switch (M.compare3min(x2, y2, z2)) {
            case -1:
                {
                    // |x1| is the smallest, use x-axis as the RHS of the cross product
                    d = 1 / MathUtil.sqrt(y2 + z2);
                    M.vec3_set(dst, 0, z1 * d, -y1 * d);
                }
                break;

            case 0:
                {
                    // |y1| is the smallest, use y-axis as the RHS of the cross product
                    d = 1 / MathUtil.sqrt(z2 + x2);
                    M.vec3_set(dst, -z1 * d, 0, x1 * d);
                }
                break;
            case 1:
                {
                    // |z1| is the smallest, use z-axis as the RHS of the cross product
                    d = 1 / MathUtil.sqrt(x2 + y2);
                    M.vec3_set(dst, y1 * d, -x1 * d, 0);
                }
                break;
        }
    }

    public static quat_arc(dst: Quat, v1: Vec3, v2: Vec3) {
        var d: number = M.vec3_dot(v1, v2);
        if (d < -1 + 1e-9) { // PI rotation, ignore v2 and set a vector perpendicular to v1
            var v = new Vec3();
            M.vec3_perp(v, v1);
            M.quat_fromVec3AndFloat(dst, v, 0);
        } else {
            var c = new Vec3();
            M.vec3_cross(c, v1, v2);

            // cos(theta/2) = sqrt((1 + cos(theta)) / 2)
            var w: number = MathUtil.sqrt((1 + d) * 0.5);

            // sin(theta/2) / sin(theta) = sin(theta/2) / (2 * sin(theta/2) * cos(theta/2))
            //                           = 1 / (2 * cos(theta/2))
            d = 0.5 / w;
            // x^2 + y^2 + z^2 = sin(theta/2)
            M.vec3_scale(c, c, d);
            M.quat_fromVec3AndFloat(dst, c, w);
        }
    }

    public static mat3_fromCols(dst: Mat3, src1: Vec3, src2: Vec3, src3: Vec3) {
        dst.init(
            src1.x, src2.x, src3.x,
            src1.y, src2.y, src3.y,
            src1.z, src2.z, src3.z
        );
    }

    public static aabb_combine(dstMin: Vec3, dstMax: Vec3, src1Min: Vec3, src1Max: Vec3, src2Min: Vec3, src2Max: Vec3) {
        this.vec3_min(dstMin, src1Min, src2Min);
        this.vec3_max(dstMax, src1Max, src2Max);
    }

    public static aabb_surfaceArea(min: Vec3, max: Vec3): number {
        var ex: number = max.x - min.x;
        var ey: number = max.y - min.y;
        var ez: number = max.z - min.z;
        return (ex * (ey + ez) + ey * ez) * 2;
    }

    public static vec3_max(dst: Vec3, src1: Vec3, src2: Vec3) {
        src1.maxTo(src2, dst);
    }

    public static vec3_min(dst: Vec3, src1: Vec3, src2: Vec3) {
        src1.minTo(src2, dst);
    }

    public static vec3_mulMat3Transposed(dst: Vec3, src1: Vec3, src2: Mat3) {
        dst.init(
            src2.e00 * src1.x + src2.e10 * src1.y + src2.e20 * src1.z,
            src2.e01 * src1.x + src2.e11 * src1.y + src2.e21 * src1.z,
            src2.e02 * src1.x + src2.e12 * src1.y + src2.e22 * src1.z
        );
    }

    public static vec3_zero(vec: Vec3) {
        vec.zero();
    }

    public static quat_fromQuat(dst: Quat, src: Quat) {
        dst.copyFrom(src);
    }

    public static quat_toQuat(src: Quat, dst: Quat) {
        dst.copyFrom(src);
    }

    public static mat3_fromEulerXyz(dst: Mat3, xyz: Vec3) {
        dst.fromEulerXyz(xyz);
    }

    public static mat3_fromMat3(dst: Mat3, src: Mat3) {
        dst.copyFrom(src);
    }

    public static mat3_toMat3(src: Mat3, dst: Mat3) {
        dst.copyFrom(src);
    }

    public static vec3_fromVec3(dst: Vec3, src: Vec3) {
        dst.copyFrom(src);
    }

    public static vec3_toVec3(dst: Vec3, src: Vec3) {
        dst.copyFrom(src);
    }

    public static transform_assign(dst: Transform, src: Transform) {
        dst.copyFrom(src);
    }

    public static mat3_zero(dst: Mat3) {
        dst.zero();
    }

    public static mat3_scaleRows(dst: Mat3, src: Mat3, x: number, y: number, z: number) {
        dst.init(
            src.e00 * x, src.e01 * x, src.e02 * x,
            src.e10 * y, src.e11 * y, src.e12 * y,
            src.e20 * z, src.e21 * z, src.e22 * z
        );
    }

    public static mat3_assign(dst: Mat3, src: Mat3) {
        dst.copyFrom(src);
    }

    public static mat3_inv(dst: Mat3, src: Mat3) {
        var d00: number = src.e11 * src.e22 - src.e12 * src.e21;
        var d01: number = src.e10 * src.e22 - src.e12 * src.e20;
        var d02: number = src.e10 * src.e21 - src.e11 * src.e20;
        var d10: number = src.e01 * src.e22 - src.e02 * src.e21;
        var d11: number = src.e00 * src.e22 - src.e02 * src.e20;
        var d12: number = src.e00 * src.e21 - src.e01 * src.e20;
        var d20: number = src.e01 * src.e12 - src.e02 * src.e11;
        var d21: number = src.e00 * src.e12 - src.e02 * src.e10;
        var d22: number = src.e00 * src.e11 - src.e01 * src.e10;
        var d: number = src.e00 * d00 - src.e01 * d01 + src.e02 * d02;
        if (d < -1e-32 || d > 1e-32) d = 1 / d;
        dst.init(
            d00 * d,
            -d10 * d,
            d20 * d,
            -d01 * d,
            d11 * d,
            -d21 * d,
            d02 * d,
            -d12 * d,
            d22 * d
        );
    }

    public static mat3_det(mat: Mat3) {
        var d00: number = mat.e11 * mat.e22 - mat.e12 * mat.e21;
        var d01: number = mat.e10 * mat.e22 - mat.e12 * mat.e20;
        var d02: number = mat.e10 * mat.e21 - mat.e11 * mat.e20;
        return mat.e00 * d00 - mat.e01 * d01 + mat.e02 * d02;
    }

    public static mat3_add(dst: Mat3, src1: Mat3, src2: Mat3) {
        src1.addTo(src2, dst);
    }

    public static mat3_addRhsScaled(dst: Mat3, src1: Mat3, src2: Mat3, num: number) {
        dst.init(
            src1.e00 + (src2.e00 * num),
            src1.e01 + (src2.e01 * num),
            src1.e02 + (src2.e02 * num),
            src1.e10 + (src2.e10 * num),
            src1.e11 + (src2.e11 * num),
            src1.e12 + (src2.e12 * num),
            src1.e20 + (src2.e20 * num),
            src1.e21 + (src2.e21 * num),
            src1.e22 + (src2.e22 * num),
        )
    }

    public static mat3_inertiaFromCOG(dst: Mat3, src: Vec3) {
        var xx: number = src.x * src.x;
        var yy: number = src.y * src.y;
        var zz: number = src.z * src.z;
        var xy: number = -src.x * src.y;
        var yz: number = -src.y * src.z;
        var zx: number = -src.z * src.x;
        dst.init(
            yy + zz, xy, zx,
            xy, xx + zz, yz,
            zx, yz, xx + yy
        );
    }

    public static mat3_scale(dst: Mat3, src: Mat3, mass: number) {
        src.scaleTo(mass, dst);
    }

    public static mat3_mul(dst: Mat3, src1: Mat3, src2: Mat3) {
        dst.init(
            src1.e00 * src2.e00 + src1.e01 * src2.e10 + src1.e02 * src2.e20,
            src1.e00 * src2.e01 + src1.e01 * src2.e11 + src1.e02 * src2.e21,
            src1.e00 * src2.e02 + src1.e01 * src2.e12 + src1.e02 * src2.e22,
            src1.e10 * src2.e00 + src1.e11 * src2.e10 + src1.e12 * src2.e20,
            src1.e10 * src2.e01 + src1.e11 * src2.e11 + src1.e12 * src2.e21,
            src1.e10 * src2.e02 + src1.e11 * src2.e12 + src1.e12 * src2.e22,
            src1.e20 * src2.e00 + src1.e21 * src2.e10 + src1.e22 * src2.e20,
            src1.e20 * src2.e01 + src1.e21 * src2.e11 + src1.e22 * src2.e21,
            src1.e20 * src2.e02 + src1.e21 * src2.e12 + src1.e22 * src2.e22
        );
    }

    public static mat3_mulLhsTransposed(dst: Mat3, src1: Mat3, src2: Mat3) {
        dst.init(
            src1.e00 * src2.e00 + src1.e10 * src2.e10 + src1.e20 * src2.e20,
            src1.e00 * src2.e01 + src1.e10 * src2.e11 + src1.e20 * src2.e21,
            src1.e00 * src2.e02 + src1.e10 * src2.e12 + src1.e20 * src2.e22,
            src1.e01 * src2.e00 + src1.e11 * src2.e10 + src1.e21 * src2.e20,
            src1.e01 * src2.e01 + src1.e11 * src2.e11 + src1.e21 * src2.e21,
            src1.e01 * src2.e02 + src1.e11 * src2.e12 + src1.e21 * src2.e22,
            src1.e02 * src2.e00 + src1.e12 * src2.e10 + src1.e22 * src2.e20,
            src1.e02 * src2.e01 + src1.e12 * src2.e11 + src1.e22 * src2.e21,
            src1.e02 * src2.e02 + src1.e12 * src2.e12 + src1.e22 * src2.e22
        );
    }

    public static mat3_transformInertia(dst: Mat3, inertia: Mat3, rotation: Mat3) {
        this.mat3_mul(dst, rotation, inertia);
        this.mat3_mulLhsTransposed(dst, dst, rotation);
    }

    public static mat3_fromQuat(dst: Mat3, quat: Quat) {
        dst.fromQuat(quat);
    }

    public static quat_mul(dst: Quat, src1: Quat, src2: Quat) {
        dst.x = src1.w * src2.x + src1.x * src2.w + src1.y * src2.z - src1.z * src2.y;
        dst.y = src1.w * src2.y - src1.x * src2.z + src1.y * src2.w + src1.z * src2.x;
        dst.z = src1.w * src2.z + src1.x * src2.y - src1.y * src2.x + src1.z * src2.w;
        dst.w = src1.w * src2.w - src1.x * src2.x - src1.y * src2.y - src1.z * src2.z;
    }

    public static quat_fromMat3(dst: Quat, mat: Mat3) {
        var e00: number = mat.e00;
        var e11: number = mat.e11;
        var e22: number = mat.e22;
        var t: number = e00 + e11 + e22;
        var s: number;
        if (t > 0) {
            s = MathUtil.sqrt(t + 1);
            dst.w = 0.5 * s;
            s = 0.5 / s;
            dst.x = (mat.e21 - mat.e12) * s;
            dst.y = (mat.e02 - mat.e20) * s;
            dst.z = (mat.e10 - mat.e01) * s;
        }
    }

    public static quat_fromVec3AndFloat(dst: Quat, vec: Vec3, num: number) {
        dst.init(vec.x, vec.y, vec.z, num);
    }

    public static vec3_length(v: Vec3): number {
        return v.length();
    }

    public static vec3_set(v: Vec3, x: number, y: number, z: number) {
        v.init(x, y, z);
    }

    public static mat3_getCol(dst: Vec3, src: Mat3, index: number) {
        src.getColTo(index, dst);
    }

    public static mat3_getRow(dst: Vec3, src: Mat3, index: number) {
        src.getRowTo(index, dst);
    }

    public static vec3_clamp(dst: Vec3, src: Vec3, min: Vec3, max: Vec3) {
        src.clampTo(min, max, dst);
    }

    public static aabb_overlap(mi1: Vec3, ma1: Vec3, mi2: Vec3, ma2: Vec3): boolean {
        return mi1.x < ma2.x && ma1.x > mi2.x &&
            mi1.y < ma2.y && ma1.y > mi2.y &&
            mi1.z < ma2.z && ma1.z > mi2.z;
    }

    public static vec3_get(vec: Vec3, index: number): number {
        switch (index) {
            case 0:
                return vec.x;
            case 1:
                return vec.y;
            case 2:
                return vec.z;
        }
    }

    public static mat3_diagonal(dst: Mat3, x: number, y: number, z: number) {
        dst.init(
            x, 0, 0,
            0, y, 0,
            0, 0, z
        );
    }

    public static vec3_compWiseMul(dst: Vec3, v1: Vec3, v2: Vec3) {
        dst.x = v1.x * v2.x;
        dst.y = v1.y * v2.y;
        dst.z = v1.z * v2.z;
    }

    public static vec3_mulHorizontal(v: Vec3): number {
        return v.x * v.y * v.z;
    }

    public static vec3_cross(dst: Vec3, v1: Vec3, v2: Vec3) {
        v1.crossTo(v2, dst);
    }

    public static vec3_scale(dst: Vec3, src: Vec3, scale: number) {
        src.scaleTo(scale, dst);
    }

    public static assert(b: boolean) {
        if (!b) {
            throw new Error("fata error!");
        }
    }

    public static vec3_mulMat3(dst: Vec3, pos: Vec3, rot: Mat3) {
        pos.mulMat3to(rot, dst);
    }

    public static vec3_isZero(v: Vec3): boolean {
        return v.isZero();
    }

    public static vec3_sub(dst: Vec3, v1: Vec3, v2: Vec3) {
        v1.subTo(v2, dst);
    }

    public static vec3_add(dst: Vec3, v1: Vec3, v2: Vec3) {
        v1.addTo(v2, dst);
    }

    public static vec3_addRhsScaled(src: Vec3, add: Vec3, scale: number): Vec3 {
        return src.addScaled(add, scale);
    }

    public static vec3_addRhsScaledEQ(_: Vec3, src: Vec3, add: Vec3, scale: number): Vec3 {
        return src.addScaledEq(add, scale);
    }

    public static vec3_addRhsScaledTo(to: Vec3, src: Vec3, add: Vec3, scale: number): Vec3 {
        return src.addScaledTo(add, scale, to);
    }

    public static error(s: string): Error {
        return new Error(s);
    }

    public static vec3_dot(v1: Vec3, v2: Vec3): number {
        return v1.dot(v2);
    }

    public static vec3_assign(dst: Vec3, src: Vec3) {
        dst.copyFrom(src);
    }

    public static transform_mul(dst: Transform, src1: Transform, src2: Transform) {
        src2._rotation.mulTo(src1._rotation, dst._rotation);
        src1._position.mulMat3to(src2._rotation, dst._position);
        dst._position.addEq(src2._position);
    }

    public static compare3min(a: number, b: number, c: number): number {
        if (a < b) {
            if (a < c) {
                // a < b, c
                return -1;
            } else {
                // c < a < b
                return 1;
            }
        } else {
            if (b < c) {
                // b < a, c
                return 0;
            } else {
                // c < b < a
                return 1;
            }
        }
    }

    public static compare3max(a: number, b: number, c: number): number {
        if (a > b) {
            if (a > c) {
                // a > b, c
                return -1;
            } else {
                // c > a > b
                return 1;
            }
        } else {
            if (b > c) {
                // b > a, c
                return 0;
            } else {
                // c > b > a
                return 1;
            }
        }
    }

    public static toFixed8(num: number): string {
        return num.toFixed(8);
    }

}
